/**
 * @file WrapFuncs.h
 * @author  
 * @brief 提供常用的封装好的函数接口
 * @version 1.0
 * @date 2020-07-27
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once

#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "data_struct/robot/Pose.h"
#include <boost/lexical_cast.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "laser_geometry/laser_geometry.hpp"
#include "data_struct/geometry/PointCloud.h"
#include "sensor_msgs/msg/laser_scan.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>

namespace behavior_controller
{
    namespace Funcs
    {

        /**
         * @brief 计算到直线的距离
         * @param 直线上的第一个点
         * @param 直线上的第二个点
         * @param 当前点
         * @return 返回当前位置到直线的距离
         */
        double DistToLine(PoseF inlier1, PoseF inlier2, PoseF outlier);

        /**
         * @brief 计算到直线的距离
         * @param 直线斜率
         * @param 直线截距
         * @param 当前点
         * @return 返回当前位置到直线的距离
         */
        double DistToLine(double line_k, double line_b, PoseF outlier);

        /**
         * @brief 计算两Pose间距
         * @param 第一个点
         * @param 第二个点
         * @return 返回两点间距
         */
        double DistToPoint(PoseF point_1st, PoseF point_2nd);

        /**
         * @brief 寻找拟合直线上的线段
         * @param 直线参数a
         * @param 直线参数b
         * @param 直线参数c
         * @param 直线上的线段端点1
         * @param 直线上的线段端点2
         * @param 碰撞悬崖第一个点
         * @param 碰撞悬崖最后一个点
         * @return
         */
        void GetInlier(const double &line_a, const double &line_b, const double &line_c,
                       const PoseF &fit_point1, const PoseF &fit_point2,
                       PoseF &start_point, PoseF &end_point);
        void GetInlier(const double &line_a, const double &line_b, const double &line_c,
                       const PoseF &fit_point,PoseF &result_point);
        /**
         * @brief 计算两个位姿之间的角度差（向量夹角）
         * @param 传入位姿1
         * @param 传入位姿2
         * @return
         */
        double GetVectorialAngle(PoseF &start_point, PoseF &end_point);

        /**
         * @brief 将参数转为-π到+π之间
         * @return
         */
        double NormalizeAngle(double raw_angle);

        /**
         * @brief 对点云数据进行直线检测,
         * 
         */
        void LineFit(PointCloud<double> &world_point_cloud, double &line_k, double &line_b);


        void LineFit(PointCloud<double> &world_point_cloud, double &line_k, double &line_b,double &r,double &x_meaan,double &y_mean);

        /**
         * @brief 对点云数据进行直线检测,
         * 
         */
        void FormScanToPC(const sensor_msgs::msg::LaserScan &msg, PointCloud<double> &dst_pc);

    } // namespace Funcs

} // namespace behavior_controller